'''负责传感器信号的获取和循迹模块的信号获取与判断运动'''
from pyb import Pin
from pyb import delay,ADC
from pyb import UART

minipyb1=UART(6,9600)#TX:C6、RX:C7

'''前面10路循迹传感器定义'''
FrontSensorPins = ['G6', 'G7', 'G8','C8','C9','A8','A13','A14','A15', 'C10']
FrontSensor = [Pin(Pin(pin), Pin.IN) for pin in FrontSensorPins]
def FrontSensorCheck():
    return [FrontSensor[p].value() for p in range(len(FrontSensorPins))]

#后面10路循迹传感器定义
#BackSensorPins = ['C11', 'C12', 'D6','D7','G14','E6','E2','E3','E4','E5']
BackSensorPins = ['C11', 'C12', 'D6','D7','G14','E6','C13','E3','E4','E5']#,'E2'
BackSensor = [Pin(Pin(pin), Pin.IN) for pin in BackSensorPins]
def BackSensorCheck():
    return [BackSensor[p].value() for p in range(len(BackSensorPins))]

#左面5路循迹传感器定义
def LeftSensorCheck():
    minipyb1.write('l')
    if minipyb1.any():
        text=eval(minipyb1.read(15))
        #print(text)
        return text

#右面10路循迹传感器定义
def RightSensorCheck():
    minipyb1.write('r')
    if minipyb1.any():
        text=eval(minipyb1.read(30))
        #print(text)
        return text

#只用前循迹运动
def OnlyFrontSensor(front):
    f=front.index(0)
    if front==[1,1,1,1,0,1,1,1,1,1] or front==[1,1,1,1,1,0,1,1,1,1]:
        return ['g',50]
    elif f<4:
        return ['m',50]#左转
    elif f>5:
        return ['n',50]#右转
    else:
        return ['g',50]

def OnlyBackSensor(back):
    if 0 not in back:#如果后循迹也脱离了就让他后退到黑线上,0不在front，back
        return ['b',300]
    else:#0在back
        b=back.index(0)
        if b>5:
            return ['k',200]#右后45
        elif b<4:
            return ['j',200]#左后45
        else:
            return ['b',200]#后退

def FrontBackCheckRun1():#前后循迹模式1
    #目前正使用
    #先同时判断前后两个循迹模块，在判断前循迹模块
    front,back=FrontSensorCheck(),BackSensorCheck()
    print(front)
    print(back)
    if 0 not in front:#0不在front
        #如果前循迹脱离黑线，用后循迹试图挽救一下
        print('Only back')
        return OnlyBackSensor(back)
    if 0 in front and 0 in back:#如果前后循迹模块都有检测到黑线
        print('front and back')
        a=front.index(0)
        b=back.index(0)
        if a-b==0:#前后某个循迹在在同一条直线
            if a==4 or a==5:#通过判断前循迹如果是中间2在同一条直线
                lrg=['g',50]

            elif a==3:#通过判断前循迹如果是左边<2在同一条直线
                lrg=['h',50]#左前45
            elif a==6:#通过判断前循迹如果是右边>2在同一条直线
                lrg=['i',50]#右前45

            elif a==2:#通过判断前循迹如果是左边<2在同一条直线
                lrg=['h',80]#左前45
            elif a==7:#通过判断前循迹如果是右边>2在同一条直线
                lrg=['i',80]#右前45

            elif a==1:#通过判断前循迹如果是左边<2在同一条直线
                lrg=['h',100]#左前45
            elif a==8:#通过判断前循迹如果是右边>2在同一条直线
                lrg=['i',100]#右前45

            elif a==0:#通过判断前循迹如果是左边<2在同一条直线
                lrg=['h',120]#左前45
            elif a==9:#通过判断前循迹如果是右边>2在同一条直线
                lrg=['i',120]#右前45
        elif a-b<0:#黑线偏左向45°
            if a==4 or a==5:#前循迹在中间的情况
                lrg=['g',50]

            elif a==3:#前循迹在左边<2的情况
                lrg=['m',20]#左转
            elif a==6:#前循迹在左边>2的情况
                lrg=['i',50]#右前45

            elif a==2:#前循迹在左边<2的情况
                lrg=['m',40]#左转
            elif a==7:#前循迹在左边>2的情况
                lrg=['i',60]#右前45

            elif a==1:#前循迹在左边<2的情况
                lrg=['m',80]#左转
            elif a==8:#前循迹在左边>2的情况
                lrg=['i',80]#右前45

            elif a==0:#前循迹在左边<2的情况
                lrg=['m',100]#左转
            elif a==9:#前循迹在左边>2的情况
                lrg=['i',100]#右前45
        elif a-b>0:#黑线偏右向45°
            #return 'n'#右转
            if a==4 or a==5:#前循迹在中间的情况
                lrg=['g',50]

            elif a==3:
                lrg=['h',20]#左前45
            elif a==6:
                lrg=['n',50]#右转

            elif a==2:
                lrg=['h',40]#左前45
            elif a==7:
                lrg=['n',60]#右转

            elif a==1:
                lrg=['h',80]#左前45
            elif a==8:
                lrg=['n',80]#右转

            elif a==0:
                lrg=['h',100]#左前45
            elif a==9:
                lrg=['n',100]#右转
        return lrg
    else:#只有前面有黑线，0不在back
        print('Only front')
        #报错的话就是其中一个模块脱离黑线，没有检测到0，
        #这里默认算它是后循迹模块脱离了黑线,以只有前循迹模块来循迹，直至后循迹模块出现信号。
        lrg=OnlyFrontSensor(front)
        return lrg

def SlowStop():#通过左面循迹模块判断是否减速或停止，停止即到达物料区
    Left=LeftSensorCheck()
    print(Left)
    return 'f'
    """
    if Left==[1,1,1,1,0] or Left==[1,1,1,0,0] or Left==[1,1,0,0,0]:
        return 's'#Slow
    elif Left==[1,0,0,0,0] or Left==[0,0,0,0,0]:
        return 'c'
    else:
        return 'f'"""



'''
if __name__=="__main__":
    while 1:
        print(CheckFBLSensor())
        delay(50)

'''
